# -*- coding: utf-8 -*-
"""
Created on Tue Aug 17 19:46:12 2021

@author: 23119
"""

import face_recognition
import cv2
import numpy as np

import RPi.GPIO as GPIO
import time

#GPIO.setmode(GPIO.BOARD)
#GPIO.setwarnings(False)
#GPIO.setup(12,GPIO.OUT,initial=GPIO.LOW)
#GPIO.setup(32,GPIO.OUT,initial=GPIO.LOW)

def setServo(servo,oldangle,newangle):#每次传舵机引脚和角度
    if oldangle<0 or oldangle>180 :
        print('out of angle')
    else:
        GPIO.setmode(GPIO.BOARD)
        GPIO.setwarnings(False)
        GPIO.setup(servo,GPIO.OUT)
        p=GPIO.PWM(servo,50)
        p.start(0)
        p.ChangeDutyCycle(2.5+newangle/18)
        oldangle=newangle
        time.sleep(0.2)
        p.stop()
        GPIO.cleanup()

oldangleA=90
oldangleB=90
setServo(12, 89, 90)#避免角度一样但是也设置了初始值
setServo(32, 89, 90)

video_capture = cv2.VideoCapture(0)
video_capture.release()
# Load a sample picture and learn how to recognize it.
#加载袁照片 并训练人脸
yuan_image = face_recognition.load_image_file("y1.jpg")

yuan_face_encoding = face_recognition.face_encodings(yuan_image)[0]

# 训练许照片
promise_image = face_recognition.load_image_file("x1.jpg")
promise_face_encoding = face_recognition.face_encodings(promise_image)[0]

# Create arrays of known face encodings and their names
#两个数组 一个用来记录已经学习的人脸  一个用来给学习过的人脸起名字 
known_face_encodings = [
    yuan_face_encoding,
    promise_face_encoding
]
known_face_names = [
    "yuan",
    "promise"
]

# Initialize some variables
face_locations = []
face_encodings = []
face_names = []
process_this_frame = True

while True:
    # Grab a single frame of video
    ret, frame = video_capture.read()
    ret=video_capture.set(3,640)
    ret=video_capture.set(4,480)

    # 对图像进行缩放 缩放为原来1/16 x轴y轴都变为原来1/4
    small_frame = cv2.resize(frame, (0, 0), fx=0.25, fy=0.25)

    # Convert the image from BGR color (which OpenCV uses) to RGB color (which face_recognition uses)
    #转换为rgb模式
    rgb_small_frame = small_frame[:, :, ::-1]

    # Only process every other frame of video to save time
    if process_this_frame:
        # Find all the faces and face encodings in the current frame of video
        face_locations = face_recognition.face_locations(rgb_small_frame)
        face_encodings = face_recognition.face_encodings(rgb_small_frame, face_locations)

        face_names = []
        print(face_locations)
        for face_encoding in face_encodings:
            # See if the face is a match for the known face(s)
            matches = face_recognition.compare_faces(known_face_encodings, face_encoding)
            name = "Unknown"

            # # If a match was found in known_face_encodings, just use the first one.
            # if True in matches:
            #     first_match_index = matches.index(True)
            #     name = known_face_names[first_match_index]

            # Or instead, use the known face with the smallest distance to the new face
            face_distances = face_recognition.face_distance(known_face_encodings, face_encoding)
            best_match_index = np.argmin(face_distances)
            if matches[best_match_index]:
                name = known_face_names[best_match_index]

            face_names.append(name)

    process_this_frame = not process_this_frame


    # Display the results
    for (top, right, bottom, left), name in zip(face_locations, face_names):
        # Scale back up face locations since the frame we detected in was scaled to 1/4 size
        top *= 4
        right *= 4
        bottom *= 4
        left *= 4

        # Draw a box around the face
        cv2.rectangle(frame, (left, top), (right, bottom), (0, 0, 255), 2)

        # Draw a label with a name below the face
        cv2.rectangle(frame, (left, bottom - 35), (right, bottom), (0, 0, 255), cv2.FILLED)
        font = cv2.FONT_HERSHEY_DUPLEX
        cv2.putText(frame, name, (left + 6, bottom - 6), font, 1.0, (255, 255, 255), 1)
        while True:
            if 60<=right -left <=100 :
                if 45 <=top-bottom<=75:
                    #不动舵机
                    pass
                else:
                    if 76<=top-bottom:
                        #B舵机向上 180方向
                        setServo(32,oldangleB,oldangleB+1)
                    else: 
                        #B舵机向下 0方向
                        setServo(32,oldangleB,oldangleB-1)
            else:
                if 101<=right-left :
                    #A舵机向右走 往180方向
                    setServo(12,oldangleA,oldangleA+1)
                    #大于180的情况放在函数中判断
                else:
                    #A舵机向左走 往0方向
                    setServo(12,oldangleA,oldangleA-1)
    # Display the resulting image
    cv2.imshow('Video', frame)

    # Hit 'q' on the keyboard to quit!
    if cv2.waitKey(1) & 0xFF == ord('q'):
        break

# Release handle to the webcam
#video_capture.release()
cv2.destroyAllWindows()
